#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.h"
#include "std_msgs/msg/string.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "geometry_msgs/msg/twist.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "rclcpp/rclcpp.hpp"
#include "mavros_msgs/msg/state.hpp"
#include "mavros_msgs/srv/command_bool.hpp"
#include "mavros_msgs/srv/set_mode.hpp"
#include "mavros_msgs/msg/position_target.hpp"
#include "string"
#include "tf2/transform_storage.h"
#include "nav_msgs/msg/odometry.hpp"
#include  "geometry_msgs/msg/vector3_stamped.hpp"
#include "yolo_detect_message/srv/service.hpp"
#include "yolo_detect_message/msg/call_server_status.hpp"
#include "yolo_detect_message/msg/move_interface.hpp"
#include "atomic"
#include "thread"
class ar_track:public rclcpp::Node
{
private:
    float MAX_ERROR = 0.3;
    float VEL_SET = 0.2;



    const int current_target_id = 0;
    bool marker_found = false;

    float last_positon_x = 0;
    float last_positon_y = 0;

    geometry_msgs::msg::Twist setPoint_row;

    std::mutex current_state_mutex;
    mavros_msgs::msg::State current_state;
    

    geometry_msgs::msg::Vector3Stamped target_position;

    rclcpp::Subscription<mavros_msgs::msg::State>::SharedPtr state_sub_;
    // current uav state

    // arming cmd 
    rclcpp::Client<mavros_msgs::srv::CommandBool>::SharedPtr arming_client_;
    // arming state
    bool arming;
    rclcpp::Client<mavros_msgs::srv::SetMode>::SharedPtr set_mode_client_;

    float init_position_x_take_off;
    float init_position_y_take_off;
    float init_position_z_take_off;
    bool flag_init_position = false;
    nav_msgs::msg::Odometry local_pose;
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr local_pos_sb;

    float position_detect_x = 0.0;
    float position_detect_y = 0.0;
    float position_detect_z = 0.0;

    rclcpp::Subscription<yolo_detect_message::msg::CallServerStatus>::SharedPtr service_status_subscriber_;

    rclcpp::Subscription<geometry_msgs::msg::Vector3Stamped>::SharedPtr ar_pos_sb;

    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr mavros_velocity_pub;

    std::thread timer_thread_; 

    bool stop_thread_;
    bool flag_arrive = false; 

    std::mutex wait_times_mutex;
    int wait_times = 0;
    bool init_flag = false;

    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Time last_request_;
    std::mutex current_velocity_mutex;
    geometry_msgs::msg::Twist current_velocity;

    bool first_deteced = false;
    std::string uav_state_topic;
    int uav_state_queue_size;

    std::string uav_location_topic;
    int uav_location_queue_size;

    std::string aruco_position_topic;
    int aruco_position_queue_size;

    std::string velocity_pub_topic;

    std::string arming_client_topic;
    std::string set_mode_client_topic;
    std::string detect_server_service;
    double max_value;

    rclcpp::CallbackGroup::SharedPtr callback_group_;
    rclcpp::Service<yolo_detect_message::srv::Service>::SharedPtr service_;
    std::mutex service_status_mutex;

    std::atomic<bool> service_status{false};


    std::string call_server_topic_name;

    int vehcile_id_;
    std::string namespaces;
    rclcpp::Publisher<yolo_detect_message::msg::MoveInterface>::SharedPtr move_pub_;

    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr cmd_pub_;
    
    // std::atomic<bool> first_arrived(false);

    std::mutex xy_is_arrived_mutex;
    bool xy_is_arrived;
public:
    ar_track(/* args */);
    ~ar_track();
    void service_status_callback(const yolo_detect_message::msg::CallServerStatus::SharedPtr msg);
    void state_callback(const mavros_msgs::msg::State::SharedPtr msg);

    void local_pos_cb(const nav_msgs::msg::Odometry::SharedPtr msg);

    void set_offboard_mode();
    void arm_drone();    

    void  ar_pos_cb(const geometry_msgs::msg::Vector3Stamped::SharedPtr msg);

    void timer_loop();


    void reset_velocity();

    void set_velocity(float x, float y);

    float clamp(float value,float max_value);

    void set_velocity_z(float z);

    void service_callback(const yolo_detect_message::srv::Service::Request::SharedPtr req,
                        const yolo_detect_message::srv::Service::Response::SharedPtr res);


    void declare_parameters()
    {
        this->declare_parameter<std::string>("subscribers.uav_state_sub.topic", "/mavros/state");
        this->declare_parameter<int>("subscribers.uav_state_sub.queue_size", 10);

        this->declare_parameter<std::string>("subscribers.uav_location_sub.topic", "/mavros/local_positon/odom");
        this->declare_parameter<int>("subscribers.uav_location_sub.queue_size", 10);

        this->declare_parameter<std::string>("subscribers.aros_pos_sb.topic", "/aruco_single/position");
        this->declare_parameter<int>("subscribers.aros_pos_sb.queue_size", 10);

        this->declare_parameter<std::string>("subscribers.velocity_pub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");





        this->declare_parameter<std::string>("client.arming_client.topic", "/mavros/cmd/arming");
        this->declare_parameter<std::string>("client.set_mode_client.topic", "/mavros/set_mode");
        
        this->declare_parameter<std::string>("client.velocity_pub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");
        this->declare_parameter<std::string>("subscribers.locaitonsub.topic", "/mavros/setpoint_velocity/cmd_vel_unstamped");

        this->declare_parameter<std::string>("services.calldetectserver.topic", "ar_track_landing_server");


        this->declare_parameter<double>("parameters.max_value", 0.5);
        this->declare_parameter<std::string>("subscribers.call_server.topic","ar_track_landing_server_topic");
        this->declare_parameter<double>("parameters.max_error", 0.2);
        this->declare_parameter<double>("parameters.vel_value", 1.0);

        this->declare_parameter<int>("parameters.vehicle_id", 1);
    
    }

    void read_parameters(){
    

        // Retrieve each parameter and store it in the corresponding variable
        this->get_parameter("subscribers.uav_state_sub.topic", uav_state_topic);
        RCLCPP_INFO(this->get_logger(), "UAV State Topic: %s", uav_state_topic.c_str());
        this->get_parameter("subscribers.uav_state_sub.queue_size", uav_state_queue_size);

        this->get_parameter("subscribers.uav_location_sub.topic", uav_location_topic);
        RCLCPP_INFO(this->get_logger(), "UAV Location Topic: %s", uav_location_topic.c_str());
        this->get_parameter("subscribers.uav_location_sub.queue_size", uav_location_queue_size);

        this->get_parameter("subscribers.aros_pos_sb.topic", aruco_position_topic);
        RCLCPP_INFO(this->get_logger(), "Aruco Position Topic: %s", aruco_position_topic.c_str());
        this->get_parameter("subscribers.aros_pos_sb.queue_size", aruco_position_queue_size);

        this->get_parameter("subscribers.velocity_pub.topic", velocity_pub_topic);
        RCLCPP_INFO(this->get_logger(), "Velocity Pub Topic: %s", velocity_pub_topic.c_str());
        this->get_parameter("client.arming_client.topic", arming_client_topic);
        RCLCPP_INFO(this->get_logger(), "Arming Client Topic: %s", arming_client_topic.c_str());
        this->get_parameter("client.set_mode_client.topic", set_mode_client_topic);
        RCLCPP_INFO(this->get_logger(), "Set Mode Client Topic: %s", set_mode_client_topic.c_str());

        this->get_parameter("services.calldetectserver.topic", detect_server_service);
        RCLCPP_INFO(this->get_logger(), "Detect Server Service: %s", detect_server_service.c_str());

        this->get_parameter("parameters.max_value", max_value);
        this->get_parameter("parameters.max_error", MAX_ERROR);
        this->get_parameter("parameters.vel_value", VEL_SET );
        this->get_parameter("subscribers.call_server.topic", call_server_topic_name);

        this->get_parameter("parameters.vehicle_id", vehcile_id_);
        // Now you can use these variables in your code
        RCLCPP_INFO(this->get_logger(), "UAV State Topic: %s, Queue Size: %d", uav_state_topic.c_str(), uav_state_queue_size);
        RCLCPP_INFO(this->get_logger(), "Max Value Parameter: %f", max_value);
        
    
    }
};


